/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/agvWorld/GeometrySrc/line.cpp
 * @Description  : 实现了Line类，表示一条线段。
 *                 提供了创建线段的多种构造函数，以及获取线段长度、斜率角和轨迹点的方法。  
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 17:45:29
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#include <math.h>
#include "tools.h"
#include "geometry.h" 

/////////////////////////////////////////////////////////////////////////////
//   Implementation of the class.

void Line::Create(const Point& start_point, const Point& end_point) 
{
    if (start_point == end_point) return;

    start_point_ = start_point;
    end_point_ = end_point;

    float dx = end_point_.x_ - start_point_.x_;
    float dy = end_point_.y_ - start_point_.y_;
    total_length_ = std::sqrt(Square(dx) + Square(dy));
	
	ASSERT(total_length_ != 0);
    if (std::fabs(total_length_) < 0.0001f) return;

    float angle = std::atan2(dy, dx);
    slant_angle_ = Angle(angle);
}

void Line::Create(const Point& start_point, const Angle& slant_angle, float total_length) 
{
    if (std::fabs(total_length) < 0.0001f) return;

    Point end_point;
    end_point.x_ = start_point.x_ + total_length * std::cos(slant_angle.radian_);
    end_point.y_ = start_point.y_ + total_length * std::sin(slant_angle.radian_);

    Create(start_point, end_point);
}

//
//   CLine: The constructor form #1.
//
Line::Line(const Point& start_point, const Point& end_point) 
{
    total_length_ = 0.0f;
    Create(start_point, end_point);
}

//
//   CLine: The constructor form #2.
//
Line::Line(const Point& start_point, const Angle& slant_angle, float total_length) 
{
    Create(start_point, slant_angle, total_length);
}

//
//   Constructor form #3
//
Line::Line(Posture& start_posture, float total_length) 
{
    Angle angle = start_posture.GetAngle();
    Create(start_posture.GetPntObject(), angle, total_length);
}

//
//   Length: Get the length of the line.
//
float Line::Length() const 
{
    return total_length_;
}

//
//   TrajFun: The trajectory generation function.
//
Point& Line::TrajFun(float cur_length) 
{
    point_.x_ = start_point_.x_ + cur_length * std::cos(slant_angle_.radian_);
    point_.y_ = start_point_.y_ + cur_length * std::sin(slant_angle_.radian_);
    return point_;
}

//
//   GetSlantAngle: Get the line's slant angle.
//
Angle& Line::SlantAngle() 
{
    return slant_angle_;
}

// The curvature generation function
float Line::CurvatureFun() const 
{
    return 0.0f;
}